#include "is_quaternion_K.h"
#include <cmath>
#include <iostream>

IsQuaternionK::IsQuaternionK()
{
}

IsQuaternionK::IsQuaternionK(kalmanType *pQA) 
{
    int i;
    for(i = 0; i < QUAT_LENGTH; i++)
    {
        lpfQuaternion[i] = *pQA;
        kcQuaternion[i] = *pQA;
        pQA++;
         
    }
}

// Constructors
IsQuaternionK::~IsQuaternionK(void)
{
    
}
/* 
 * This function will normalize the quaternion.
 * q = q/norm(q);
 * q is a 4x1 vector. 
*/
void IsQuaternionK::normalizeQuatK(kalmanType IsQuaternionK[4])
{
    int i;
    kalmanType tmpQuat[4];
    kalmanType* pIsQuaternionK = IsQuaternionK;
    //cout<<"DEBUG: Entering to normalizeQuatK Function."<<endl;
    for(i=0; i < QUAT_LENGTH; i++)
    {
        tmpQuat[i] = *pIsQuaternionK;
        pIsQuaternionK++;
    }

    // Now we normalize this 'tmpQuat'
    double w = tmpQuat[0];
    double x = tmpQuat[1];
    double y = tmpQuat[2];
    double z = tmpQuat[3];
    double norm = sqrt(pow(w,2) + pow(x,2) + pow(y,2) + pow(z,2));
    tmpQuat[0] = w/norm; tmpQuat[1] = x/norm; 
    tmpQuat[2] = y/norm; tmpQuat[3] = z/norm;
    /*
    cout<<"DEBUG: Print this quaternion :"<<tmpQuat[0]<<"\t"<<tmpQuat[1]
        <<"\t"<<tmpQuat[2]<<"\t"<<tmpQuat[3];
    */
    // copy this normalized quaternion into the original quat
    for(i = 0; i < QUAT_LENGTH; i++)
    {
        IsQuaternionK[i] = tmpQuat[i];
    }
#if 0
    cout<<"DEBUG: Print this quaternion :"<<IsQuaternionK[0]<<"\t"<<IsQuaternionK[1]
        <<"\t"<<IsQuaternionK[2]<<"\t"<<IsQuaternionK[3];
#endif
}
/* 
 * This function will calculate the inverse of a quaternion.
 * The quaternion inverse has the form of 
 * q^-1 = [w -x -y -z]./norm(q)
 */

void IsQuaternionK::invertQuaternion(kalmanType IsQuaternionK[QUAT_LENGTH])
{
    int i;
    kalmanType tmpQuat[4];
    kalmanType* pIsQuaternionK = IsQuaternionK;
    for(i=0; i < QUAT_LENGTH; i++)
    {
        tmpQuat[i] = *pIsQuaternionK;
        pIsQuaternionK++;
    }
    double w = tmpQuat[0];
    double x = tmpQuat[1];
    double y = tmpQuat[2];
    double z = tmpQuat[3];
    // calculate the norm of this vector
    double norm = sqrt(pow(w,2) + pow(x,2) + pow(y,2) + pow(z,2));
    tmpQuat[0] = w/norm; tmpQuat[1] = -x/norm; 
    tmpQuat[2] = -y/norm; tmpQuat[3] = -z/norm;
    // copy this into original quat
    for(i = 0; i < QUAT_LENGTH; i++)
    {
        IsQuaternionK[i] = tmpQuat[i];
    }
}

/* ABOUT :
 * This function will compute a 3-D rotation matrix given a rotation quaternion
 * The quaternion encode the 3-D rotation needed to rotate a reference frame [X Y Z]'  * to a body-centered frame [x y z]' (e.g. camera). The rotation matrix transform the  * coordinate in XYZ to xyz; i.e.:
 *   [x y z]' = m * [X Y Z]'
 * We use the aerospace Z-Y-X yaw-pitch-roll angle 
 * [psi theta phi]'. By convention, the x is along the "forward" direction,
 * y is to the "right" direction, and z is pointing "downward".
 * The rotation quaternion is assumed to have norm 1.
 * RETURN : pointer to array of 9 elements representing matrix.
 */
void IsQuaternionK::quatToRotationMat(kalmanType *pIsQuaternionK)
{
    kalmanType* pMyRotationMatrix = myRotationMatrix;
    kalmanType tmpQuat[4];
    for(int i=0; i < QUAT_LENGTH; i++)
    {
        tmpQuat[i] = *pIsQuaternionK;
        pIsQuaternionK++;
    }
    // Normalize this quaternion.
    IsQuaternionK::normalizeQuatK(tmpQuat);
    double w = tmpQuat[0];
    double x = tmpQuat[1];
    double y = tmpQuat[2];
    double z = tmpQuat[3];
    double norm = sqrt(pow(w,2) + pow(x,2) + pow(y,2) + pow(z,2));
    if(norm == 1){
       // cout<<"\tCHECK : Quaternion is notmalized. Continuing..."<<endl;
    }
    else{
        //cout<<"\tDEBUG: Normalising this quaternion..."<<endl;
        IsQuaternionK::normalizeQuatK(tmpQuat);
    }
    // computing rotation matrix
    double a, b, c, d;
    a = w; b = x; c = y; d = z;
    // column 1
    *(pMyRotationMatrix + 0) = pow(a,2) + pow(b,2) - pow(c,2) - pow(d,2); 
    *(pMyRotationMatrix + 3) = 2*(b*c - a*d); 
    *(pMyRotationMatrix + 6) = 2*(b*d + a*c);    
    // column 2
    *(pMyRotationMatrix + 1) = 2*(b*c + a*d);    
    *(pMyRotationMatrix + 4) = pow(a,2) - pow(b,2) + pow(c,2) - pow(d,2);
    *(pMyRotationMatrix + 7) = 2*(c*d - a*b);        
    // column 3
    *(pMyRotationMatrix + 2) = 2*(b*d - a*c);    
    *(pMyRotationMatrix + 5) = 2*(c*d + a*b);
    *(pMyRotationMatrix + 8) = pow(a,2) - pow(b,2) - pow(c,2) + pow(d,2); 
#if 0
    printf("\n--DEBUG: Printing rotation matrix.\n");
    for(int i=0; i<9; i++)
    {
        cout<<*pMyRotationMatrix<<endl;
        pMyRotationMatrix++;
    }
#endif
    pMyRotationMatrix = &myRotationMatrix[0];
   
}

/* ABOUT : This function computes the hamilton Product of two quternions.
 * RETURN : pointer to an array of 4 elements.
 */
kalmanType* IsQuaternionK::hamiltonionProduct(kalmanType *plpfQuaternion, kalmanType *pthIsQuaternionK)
{
    //cout<<"DEBUG: Entering hamitonionProduct function."<<endl;
    kalmanType myHamiltonProduct[QUAT_LENGTH]; // Hamilton product 
    kalmanType* pMyHamiltonProduct = myHamiltonProduct; // Pointer to array    
    kalmanType tmpCurQuat[QUAT_LENGTH];
    kalmanType tmpPrevQuat[QUAT_LENGTH];

    for(int i=0; i < QUAT_LENGTH; i++)
    {
        tmpCurQuat[i] = *plpfQuaternion;
        tmpPrevQuat[i] = *pthIsQuaternionK;
        plpfQuaternion++;
        pthIsQuaternionK++;
    }
    // normalize these quaternions.
    IsQuaternionK::normalizeQuatK(tmpCurQuat);
    IsQuaternionK::normalizeQuatK(tmpPrevQuat);

    // Creating intermediate varibles to calculate the matrix.
    kalmanType a1, b1, c1, d1;
    kalmanType a2, b2, c2, d2;
    // Current Quaternions
    a1 = tmpCurQuat[0]; b1 = tmpCurQuat[1];   
    c1 = tmpCurQuat[2]; d1 = tmpCurQuat[3];   
    // Previous quaternion
    a2 = tmpPrevQuat[0]; b2 = tmpPrevQuat[1];   
    c2 = tmpPrevQuat[2]; d2 = tmpPrevQuat[3];
    
    myHamiltonProduct[0] = a1*a2 - b1*b2 - c1*c2 - d1*d2;
    myHamiltonProduct[1] = a1*b2 + b1*a2 + c1*d2 - d1*c2;
    myHamiltonProduct[2] = a1*c2 - b1*d2 + c1*a2 + d1*b2;
    myHamiltonProduct[3] = a1*d2 + b1*c2 - c1*b2 + d1*a2;
#if 0
    printf("\n--DEBUG:Printing Hamiltion Product.\n");
    for(int i=0; i<QUAT_LENGTH; i++)
    {
      //  cout<<*pMyHamiltonProduct<<endl;
        pMyHamiltonProduct++;
    }
#endif
    return pMyHamiltonProduct;
}

